#include "mainwindow.h"
#include "ui_mainwindow.h"
#include "logger.h"

MainWindow::MainWindow(QWidget *parent) :
    QMainWindow(parent),
    ui(new Ui::MainWindow)
{
    if(false == mWorkerRunnig){
        LOG(INFO) << "start mWorkerRunnig thread\n";
        mWorkerRunnig = true;
        worker = std::thread(&MainWindow::workThreadFun, this);
    }

    ui->setupUi(this);
}

MainWindow::~MainWindow()
{
    delete ui;
}

void MainWindow::on_pushButton_clicked()
{
    LOG(INFO) << "Button VERSION_GET_DRAWING_GUN_POINT start";
    mWorkerProcMutex.lock();
    mWorkerCurTask = VERSION_GET_DRAWING_GUN_POINT;
    mWorkerRecvStatus = true;
    mWorkerProcMutex.unlock();
    mWorkercond.notify_all();
    LOG(INFO) << "Button VERSION_GET_DRAWING_GUN_POINT end";
}

void MainWindow::workThreadFun()
{
    LOG(INFO) << "workThreadFun tid:"<< std::this_thread::get_id() <<" has start.\n";
    while(mWorkerRunnig)
    {
        std::unique_lock<std::mutex> lk(mWorkerProcMutex);
        mWorkercond.wait(lk, [this]{return mWorkerRecvStatus;});
        mWorkerRecvStatus = false;
        if(!mWorkerRunnig) {
            lk.unlock();
            break;
        }

        switch (mWorkerCurTask) {
        case VERSION_GET_DRAWING_GUN_POINT:
        {
            int project_id = 1;
            LOG(INFO) << "do VERSION_GET_DRAWING_GUN_POINT start.";

            //启动视觉工程
            MECH_VERSION_POSE joint_pos={5.18, 14.52, 4.03, 0.09, 72.44, 5.15};
            MECH_VERSION_POSE tcp_pose={549.56, 50.0, 647.01, 180.0, -1.0, 180.0};
            if(-1 == mm_client.mm_cmd_run_version(project_id,joint_pos,tcp_pose)){
                LOG(ERROR) << " mm_client.mm_cmd_run_version err!";
                break;
            }

            std::vector<MECH_VERSION_POSE> poses;
            if(-1 == mm_client.mm_cmd_receive_verion(project_id, poses)){
                LOG(ERROR) << " mm_client.mm_cmd_receive_verion err!";
                break;
            }

            LOG(INFO) << "--- print  poses start --- " ;
            for(int i=0; i<poses.size(); i++){
                LOG(INFO) << "pose num " << i;
                print_mech_version_pose(poses[i]);
            }
            LOG(INFO) << "--- print  poses end --- " ;

            //获取视觉点
            LOG(INFO) << "do VERSION_GET_DRAWING_GUN_POINT end.";
            break;
        }
        case ROBOT_GET_JOINT_POS:
        {
            LOG(INFO) << "do ROBOT_GET_JOINT_POS start.";
            if(ELITE_SDK_PARSE_JSON_OK != elite_robot.get_joint_pos(joint_pos.p)){
                LOG(ERROR) << "elite_robot get_joint_pos err!";
                break;
            }

            LOG(INFO) << "get_joint_pos is:";
            print_mech_version_pose(joint_pos);

            LOG(INFO) << "do ROBOT_GET_JOINT_POS end.";
            break;
        }
        case ROBOT_GET_TCP_POSE:
        {
            LOG(INFO) << "do ROBOT_GET_TCP_POSE start.";
            if(ELITE_SDK_PARSE_JSON_OK != elite_robot.get_tcp_pose(tcp_pose.p)){
                LOG(ERROR) << "elite_robot get_joint_pos err!";
                break;
            }

            LOG(INFO) << "get_tcp_pose is:";
            print_mech_version_pose(tcp_pose);

            LOG(INFO) << "do ROBOT_GET_JOINT_POS end.";
            break;
        }
        case GRIPPER_RETURN_ORIGIN:
        {
            LOG(INFO) << "do GRIPPER_RETURN_ORIGIN start.";
            rm_gripper.return_to_original();
            LOG(INFO) << "do GRIPPER_RETURN_ORIGIN end.";
            break;
        }
        case GRIPPER_NUM0_PUSH_MOVE:
        {
            LOG(INFO) << "do GRIPPER_NUM0_PUSH_MOVE start.";
            rm_gripper.trigger_num0_pushing_motion();
            LOG(INFO) << "do GRIPPER_NUM0_PUSH_MOVE end.";
            break;
        }
        case GRIPPER_NUM0_SET_PARAMS:
        {
            LOG(INFO) << "do GRIPPER_NUM0_SET_PARAMS start.";
            rm_gripper.set_num0_pushing_motion();
            LOG(INFO) << "do GRIPPER_NUM0_SET_PARAMS end.";
            break;
        }

        default:
        {
            LOG(ERROR) << "Shouldn't be here!";
            break;
        }
            LOG(INFO) << "Task done!Waitting for next task..." << std::endl;
        }
    }
    LOG(INFO) << "work thread tid:"<< std::this_thread::get_id() <<" has exit.\n";
}

void MainWindow::print_mech_version_pose(MECH_VERSION_POSE mech_pose)
{
    LOG(INFO) <<mech_pose.p[0]<<","<<mech_pose.p[1]<<","<<mech_pose.p[2]<<","\
                             <<mech_pose.p[3]<<","<<mech_pose.p[4]<<","<<mech_pose.p[5];
}

int MainWindow::deal_input_pose_str(std::string input_str, PoseInfo &pose_info)
{
    size_t pos = input_str.rfind(';');
    // 如果字符在字符串中存在且位于字符串末尾，则返回true
    if (pos == std::string::npos || pos != input_str.size() - 1) {
        LOG(ERROR) << "The string does not end with the character ';'";
        return -1;
    }

    std::vector<std::string> reslut = stringSplit(input_str, ',');
    if(reslut.size()!=9){
        LOG(ERROR) << "The input str error!"<< "reslut.size()= "<<reslut.size()<< std::endl;
        return -1;
    }
    for(int i=0;i<6;i++){
        pose_info.pose[i] = std::stod(reslut[i]);
    }
    pose_info.speed = std::stod(reslut[6]);
    pose_info.acc =std::stod(reslut[7]);
    pose_info.dec =std::stod(reslut[8]);
    return 0;
}

void MainWindow::on_pushButton_2_clicked()
{
    if(-1 == mm_client.init()){
        LOG(ERROR) << "mm client init err!";
        return;
    }
    ui->pushButton_2->setText(tr("已连接"));
}

void MainWindow::closeEvent(QCloseEvent *event)
{
    LOG(INFO) << "closeEvent start!";

    if(worker.joinable())
    {
        mWorkerProcMutex.lock();
        mWorkerRunnig = false;
        mWorkerRecvStatus = true;
        mWorkerProcMutex.unlock();

        mWorkercond.notify_all();
        worker.join();
    }
    LOG(INFO) << "closeEvent end!";

}

void MainWindow::on_pushButton_3_clicked()
{
    if(false == elite_robot.init()){
        LOG(ERROR) << "elite_robot init failed!!!\n";
        return;
    }
    ui->pushButton_3->setText(tr("已连接"));
}

void MainWindow::on_pushButton_4_clicked()
{
    LOG(INFO) << "Button ROBOT_GET_JOINT_POS start";
    mWorkerProcMutex.lock();
    mWorkerCurTask = ROBOT_GET_JOINT_POS;
    mWorkerRecvStatus = true;
    mWorkerProcMutex.unlock();
    mWorkercond.notify_all();
    LOG(INFO) << "Button ROBOT_GET_JOINT_POS end";
}

void MainWindow::on_pushButton_5_clicked()
{
    LOG(INFO) << "Button ROBOT_GET_TCP_POSE start";
    mWorkerProcMutex.lock();
    mWorkerCurTask = ROBOT_GET_TCP_POSE;
    mWorkerRecvStatus = true;
    mWorkerProcMutex.unlock();
    mWorkercond.notify_all();
    LOG(INFO) << "Button ROBOT_GET_TCP_POSE end";
}

void MainWindow::on_pushButton_6_clicked()
{
    LOG(INFO) << "Start of move by JPs!";
    EliteMoveByjoint move_by_joint;

    PoseInfo pose_info={};
    std::string deal_str = ui->lineEdit->text().toStdString();
    if(-1 == deal_input_pose_str(deal_str,pose_info)){
        LOG(ERROR) << "Deal input pose str error!";
        return;
    }
    MECH_VERSION_POSE mm_pose;
    memcpy(mm_pose.p, pose_info.pose, sizeof(Param6D));
    print_mech_version_pose(mm_pose);
    LOG(INFO) << " speed = " << pose_info.speed;
    LOG(INFO) << " acc = " << pose_info.acc;
    LOG(INFO) << " dec = " << pose_info.dec;

    bool reslut;
    memcpy(&move_by_joint.joint_pos, pose_info.pose, sizeof(Param6D));
    move_by_joint.speed = pose_info.speed;

    elite_robot.mov_joint_pos(move_by_joint,reslut);
    LOG(INFO) << "End of move by JPs!";
}

void MainWindow::on_pushButton_7_clicked()
{
    LOG(INFO) << "Start of move by Tcp!";
    if(ui->lineEdit_2->text() == ""){
        LOG(ERROR) << "Input str is null!";
        return;
    }

    PoseInfo pose_info={};
    std::string deal_str = ui->lineEdit_2->text().toStdString();
    if(-1 == deal_input_pose_str(deal_str,pose_info)){
        LOG(ERROR) << "Deal input pose str error!";
        return;
    }
    MECH_VERSION_POSE mm_pose;
    memcpy(mm_pose.p, pose_info.pose, sizeof(Param6D));
    print_mech_version_pose(mm_pose);
    LOG(INFO) << " speed = " << pose_info.speed;
    LOG(INFO) << " acc = " << pose_info.acc;
    LOG(INFO) << " dec = " << pose_info.dec;
#if 0
    bool reslut;
    EliteInverseKinematic inverse_kinematic;
    memcpy(inverse_kinematic.target_pose, pose_info.pose, sizeof(Param6D));
    if(ELITE_SDK_PARSE_JSON_OK !=elite_robot.mov_tcp_pose(inverse_kinematic, reslut)){
        LOG(ERROR) << "Communicate mov tcp pose pose error!";
        return;
    }

    if(!reslut){
        LOG(ERROR) << "Opt mov tcp pose pose error!";
        return;
    }
#endif
    LOG(INFO) << "End of move by Tcp!";
}

void MainWindow::on_pushButton_13_clicked()
{
    LOG(INFO) << "Button GRIPPER_NUM0_PUSH_MOVE start";
    mWorkerProcMutex.lock();
    mWorkerCurTask = GRIPPER_NUM0_PUSH_MOVE;
    mWorkerRecvStatus = true;
    mWorkerProcMutex.unlock();
    mWorkercond.notify_all();
    LOG(INFO) << "Button GRIPPER_NUM0_PUSH_MOVE end";
}

void MainWindow::on_pushButton_11_clicked()
{
    if(-1 == rm_gripper.init()){
        LOG(ERROR) << " rm gripper init err!";
        return;
    }
    ui->pushButton_11->setText(tr("已连接"));

    //rm_gripper.set_num0_pushing_motion();
}

void MainWindow::on_pushButton_12_clicked()
{
    LOG(INFO) << "Button GRIPPER_RETURN_ORIGIN start";
    mWorkerProcMutex.lock();
    mWorkerCurTask = GRIPPER_RETURN_ORIGIN;
    mWorkerRecvStatus = true;
    mWorkerProcMutex.unlock();
    mWorkercond.notify_all();
    LOG(INFO) << "Button GRIPPER_RETURN_ORIGIN end";
}

void MainWindow::on_pushButton_14_clicked()
{
    LOG(INFO) << "Button rm gripper stop start";
    mWorkerProcMutex.lock();
    mWorkerCurTask = GRIPPER_NUM0_SET_PARAMS;
    mWorkerRecvStatus = true;
    mWorkerProcMutex.unlock();
    mWorkercond.notify_all();
    LOG(INFO) << "Button rm gripper stop end";
}


void MainWindow::on_pushButton_10_clicked()
{
    bool reslut;
    LOG(INFO) << "Elite robot stop start";
    elite_robot.stop(reslut);
    LOG(INFO) << "Elite robot stop end";
}
